' Test_DaniSCARA1

Button 1 Text Swipe_ARM0
Button 2 Text Swipe_ARM1
Button 3 Text Swipe_ARM2
Button 4 Text PIckPlace
Button 5 Text HOMING
Button 6 Text HAL

Stop

Label PIckPlace
	Slot 1 = 10			'Move to pick, Arm0
	Gosub WaitAllMotOrs
	Slot 3 = -100		'Move to pick, Arm1
	Gosub WaitAllMotOrs
	Slot 8 = 200		'Open grip
	Wait Seconds 0.2
	Slot 5 = 200		'Move arm2 DOWN
	Gosub WaitAllMotOrs
	Slot 8 = 900		'Close grip
	Wait Seconds 0.2
	Slot 5 = 0			'Move arm2 UP
	Gosub WaitAllMotOrs
	Slot 3 = -50		'Move to place, Arm1
	Gosub WaitAllMotOrs     
	Slot 5 = 200		'Move to place, Arm2
	Gosub WaitAllMotOrs
	Slot 1 = 250		'Move to pick, Arm0
	Gosub WaitAllMotOrs
	Slot 5 = 100		'Move arm2 DOWN
	Gosub WaitAllMotOrs
	Slot 8 = 200		'Open grip
	Wait Seconds 0.2
	Slot 5 = 0			'Move arm2 UP
	Gosub WaitAllMotOrs
	Slot 8 = 900		'Close grip
	Wait Seconds 0.2
Goto PIckPlace

Label Swipe_ARM0
	Slot 1 = 10
	Gosub WaitAllMotOrs
	Wait Seconds 0.7
	Slot 1 = 200
	Gosub WaitAllMotOrs
	Wait Seconds 0.7	
Goto Swipe_ARM0

Label Swipe_ARM1
	Slot 3 = -20
	Gosub WaitAllMotOrs	
	Wait Seconds 0.8	
	Slot 3 = -300
	Gosub WaitAllMotOrs	
	Wait Seconds 0.8	
Goto Swipe_ARM1

Label Swipe_ARM2
	Slot 5 = 20
	Gosub WaitAllMotOrs 
	Slot 5 = 200
	Gosub WaitAllMotOrs
Goto Swipe_ARM2

Label HOMING
	Slot 1 = Reset
	Wait Seconds 0.02
	Slot 1 = 0
	Wait Seconds 0.02
	Label Counter1		
		Slot(1) = Slot(1) - 1
		Wait Seconds 0.005
		If Slot(1) < -100 		'1000
			Slot 1 = Reset
			Wait Seconds 0.02
			Slot 1 = 0
			Wait Seconds 0.02
			Goto Homing2
		EndIf
	Goto Counter1
	
Label Homing2
	Slot 3 = Reset
	Wait Seconds 0.02
	Slot 3 = 0
	Wait Seconds 0.02
	Label Counter2		
		Slot(3) = Slot(3) + 1
		Wait Seconds 0.005
		If Slot(3) > 100		'1000 
			Slot 3 = Reset
			Wait Seconds 0.02
			Slot 3 = 0
			Wait Seconds 0.02
			Goto Homing3
		EndIf
	Goto Counter2
	
Label Homing3
	Slot 5 = Reset
	Wait Seconds 0.02
	Slot 5 = 0
	Wait Seconds 0.02
	Label Counter3		
		Slot(5) = Slot(5) - 1
		Wait Seconds 0.005
		If Slot(5) < -50		'500 
			Slot 5 = Reset
			Wait Seconds 0.02
			Slot 5 = 0
			Stop
		EndIf
	Goto Counter3
	
Label HAL
	Load Theremino_HAL.exe
Stop

Label WaitAllMotOrs
	If Slot(2) = 0 And Slot(4) = 0 And Slot(6) = 0
		Return
	EndIf
	Wait Seconds 0.001
Goto WaitAllMotOrs

